/**
 * @file ebex_util.h
 *
 * @date Oct 21, 2011
 * @author seth
 *
 * @brief This file is part of EBEX State Estimator, created for the EBEX project
 *
 * This software is copyright (C) 2011 Columbia University
 *
 * EBEX State Estimator is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * EBEX State Estimator is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with EBEX State Estimator; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#ifndef EBEX_UTIL_H_
#define EBEX_UTIL_H_

/*
 * _GNU_SOURCE includes the _POSIX_SOURCE and _ISOC99_SOURCE
 * C extensions, which are normally fairly useful!
 * This has to be defined prior to the inclusion of any of the
 * standard C headers.
 */
#if defined(__GNUC__) && !defined(_GNU_SOURCE)
# define _GNU_SOURCE
#endif

#include <stdint.h>
#include <limits.h>
#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>


/*
 * Useful constants
 */
#define DBL_TINY            (1.0e-8)
#define BOLTZMANN_FACTOR	(1.3806505e-23)

#define ISDBL_TINY(x)		(fabs(x) < DBL_TINY)


#ifdef MIN
#	undef MIN
#endif
#ifdef MAX
#	undef MAX
#endif

#define MIN(x,y) ({				\
        typeof(x) _x = (x);		\
        typeof(y) _y = (y);		\
        _x < _y ? _x : _y; })

#define MAX(x,y) ({				\
        typeof(x) _x = (x);		\
        typeof(y) _y = (y);		\
        _x > _y ? _x : _y; })

#ifdef CLAMP
#	undef CLAMP
#endif

#define CLAMP(x, low, high) ({	\
	typeof(x) _x = (x);			\
	typeof(x) _low = (low);		\
	typeof(x) _high = (high);	\
	((_x > _high) ? _high : 	\
	((_x < _low) ? _low : _x)); \
	})

#define	EBEX_SUCCESS	0
#define	EBEX_FAIL		-1

static __always_inline
uint32_t ezip_ilog2(const uint32_t m_val)
{
    uint32_t ret;
    __asm__
    ("\
        xorl    %0, %0      ;\
        decl    %0          ;\
        bsrl    %1, %0      ;\
        "
        : "=&r" (ret)
        : "mr" (m_val)
        : "cc"
    );
    return ret;
}

#define IS_POW_TWO(x) (!((x) & ((x)-1)))
static inline size_t next_pow_two(size_t m_size)
{
    uint32_t shift_val;
    if (IS_POW_TWO(m_size)) return m_size;
    if ((sizeof(size_t) > 4) && ((shift_val = ezip_ilog2(m_size >> 32))))
    {
        return (((size_t)1)<<(shift_val + 33));
    }
    return (((size_t)1)<<(ezip_ilog2(m_size) + 1));
}


#define die(X)	do {						\
                printf("FATAL ERROR: %s\nin %s at \"%s\" line %d\n",	\
                		(X),					\
                __func__,			\
                __FILE__,				\
                __LINE__);				\
                fflush(NULL);				\
                abort();				\
		} while(0)

#  define dief(format, ...)	do {			\
			printf("FATAL ERROR: ");		\
			printf(format , ##__VA_ARGS__);		\
			printf("\nin %s at \"%s\" line %d\n",	\
			__func__,			\
			__FILE__,				\
			__LINE__);				\
			fflush(NULL);				\
			abort();                           \
			} while(0)


#endif /* EBEX_UTIL_H_ */
